D=input(':')
Y=input('Ӱ:')/1000
f=input(':')/1000
C=input('ʼֵ:')
J=input('ⷽλǳʼֵ:')
i=size(Y,1);
Y=[Y;Y(1,:)];
D=[D;D(1,:)]; 
Xs=0;
Ys=0;
for j=1:i
    m=sqrt( ((D(j,1)-D(j+1,1)).^2+(D(j,2)-D(j+1,2)).^2) /((Y(j,1)-Y(j+1,1)).^2+(Y(j,2)-Y(j,2)).^2))/(i-1);
    Xs=Xs+D(j,1)/i;
    Ys=Ys+D(j,2)/i;
end
 Zs=m*f;
for j=1:1000
R(1,1)=cos(J(1))*cos(J(3))-sin(J(1))*sin(J(2))*sin(J(3));
R(1,2)=-cos(J(1))*sin(J(3))-sin(J(1))*sin(J(2))*cos(J(3));
R(1,3)=-sin(J(1))*cos(J(2));
R(2,1)=cos(J(2))*sin(J(3));
R(2,2)=cos(J(2))*cos(J(3));
R(2,3)=-sin(J(2));
R(3,1)=sin(J(1))*cos(J(3))+cos(J(1))*sin(J(2))*sin(J(3));
R(3,2)=-sin(J(1))*sin(J(3))+cos(J(1))*sin(J(2))*cos(J(3));
R(3,3)=cos(J(1))*cos(J(2));
for n=1:i
Xp(n)=R(1,1)*(D(n,1)-Xs)+R(2,1)*(D(n,2)-Ys)+R(3,1)*(D(n,3)-Zs);
Yp(n)=R(1,2)*(D(n,1)-Xs)+R(2,2)*(D(n,2)-Ys)+R(3,2)*(D(n,3)-Zs);
Zp(n)=R(1,3)*(D(n,1)-Xs)+R(2,3)*(D(n,2)-Ys)+R(3,3)*(D(n,3)-Zs);
xc(n)=-f*Xp(n)/Zp(n);
yc(n)=-f*Yp(n)/Zp(n);
A(2*n-1,1)=(R(1,1)*f+R(1,3)*xc(n))/Zp(n);
A(2*n-1,2)=(R(2,1)*f+R(2,3)*xc(n))/Zp(n);
A(2*n-1,3)=(R(3,1)*f+R(3,3)*xc(n))/Zp(n);
A(2*n,1)=(R(1,2)*f+R(1,3)*yc(n))/Zp(n);
A(2*n,2)=(R(2,2)*f+R(2,3)*yc(n))/Zp(n);
A(2*n,3)=(R(3,2)*f+R(3,3)*yc(n))/Zp(n);
A(2*n-1,4)=yc(n)*sin(J(2))-(xc(n)/f*(xc(n)*cos(J(3))-yc(n)*sin(J(3)))+f*cos(J(3)))*cos(J(2));
A(2*n-1,5)=-f*sin(J(3))-xc(n)*(xc(n)*sin(J(3))+yc(n)*cos(J(3)))/f;
A(2*n-1,6)=yc(n);
A(2*n,4)=-xc(n)*sin(J(2))- (yc(n)/f*(xc(n)*cos(J(3))-yc(n)*sin(J(3)))-f*sin(J(3)))*cos(J(2));
A(2*n,5)=  -f*cos(J(3))-yc(n)*(xc(n)*sin(J(3))+yc(n)*cos(J(3)))/f;
A(2*n,6)=-xc(n);
L(2*n-1,1)=Y(n,1)-xc(n);
L(2*n,1)=Y(n,2)-yc(n);
end 
X=inv(A'*A)*A'*L;
    Xs=Xs+X(1);
    Ys=Ys+X(2);
    Zs=Zs+X(3);
    J(1)=J(1)+X(4);
    J(2)=J(2)+X(5);
    J(3)=J(3)+X(6);
  M=max(abs(X));
 if M<=0.001
  Xs
  Ys
  Zs
  R
  break;
 end
end


    

